from rclpy.node import Node
from rcl_interfaces.msg import SetParametersResult
from std_msgs.msg import String

class SimpleParamPub(Node):
    def __init__(self):
        super().__init__('param_node')

        # 声明参数并设置默认值
        self.declare_parameter('publish_frequency', 1.0)  # 发布频率(Hz)
        self.declare_parameter('message_prefix', 'Hello')  # 消息前缀
        self.declare_parameter('enable_debug', False)  # 调试模式开关

        # 获取初始参数值
        freq = self.get_parameter('publish_frequency').value
        prefix = self.get_parameter('message_prefix').value
        debug = self.get_parameter('enable_debug').value

        # 创建发布者
        self.publisher_ = self.create_publisher(String, 'data_topic', 10)

        # 创建定时器（基于参数动态设置频率）
        self.timer_period = 1.0 / freq
        self.timer = self.create_timer(self.timer_period, self.timer_callback)

        # 添加参数变更回调
        self.add_on_set_parameters_callback(self.param_change_callback)

        # 初始化计数器
        self.counter = 0

        self.get_logger().info(f'节点已启动，发布频率: {freq}Hz，消息前缀: "{prefix}"')
        if debug:
            self.get_logger().warning('调试模式已启用!')

    def timer_callback(self):
        """定时发布数据"""
        prefix = self.get_parameter('message_prefix').value
        msg = String()
        msg.data = f'{prefix} #{self.counter}'
        self.publisher_.publish(msg)
        self.get_logger().info(f'发布: {msg.data}')
        self.counter += 1

    def param_change_callback(self, params):
        """参数变更回调函数"""
        result = SetParametersResult()
        result.successful = True

        for param in params:
            if param.name == 'publish_frequency':
                # 频率变更时重新设置定时器
                new_freq = param.value
                self.timer.cancel()
                self.timer_period = 1.0 / new_freq
                self.timer = self.create_timer(self.timer_period, self.timer_callback)
                self.get_logger().info(f'更新发布频率: {new_freq}Hz')

            elif param.name == 'message_prefix':
                self.get_logger().info(f'更新消息前缀: "{param.value}"')

            elif param.name == 'enable_debug':
                status = "启用" if param.value else "禁用"
                self.get_logger().warning(f'调试模式{status}')
            else:
                self.get_logger().warning(f'未知参数变更: {param.name}')
        return result
